//M*//////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "precomp.hpp"

#undef INFINITY
#define INFINITY 10000
#define OCCLUSION_PENALTY 10000
#define OCCLUSION_PENALTY2 1000
#define DENOMINATOR 16
#undef OCCLUDED
#define OCCLUDED CV_STEREO_GC_OCCLUDED
#define CUTOFF 1000
#define IS_BLOCKED(d1, d2) ((d1) > (d2))

typedef struct GCVtx {
	GCVtx* next;
	int parent;
	int first;
	int ts;
	int dist;
	short weight;
	uchar t;
}
GCVtx;

typedef struct GCEdge {
	GCVtx* dst;
	int next;
	int weight;
}
GCEdge;

typedef struct CvStereoGCState2 {
	int Ithreshold, interactionRadius;
	int lambda, lambda1, lambda2, K;
	int dataCostFuncTab[CUTOFF+1];
	int smoothnessR[CUTOFF*2+1];
	int smoothnessGrayDiff[512];
	GCVtx** orphans;
	int maxOrphans;
}
CvStereoGCState2;

// truncTab[x+255] = MAX(x-255,0)
static uchar icvTruncTab[512];
// cutoffSqrTab[x] = MIN(x*x, CUTOFF)
static int icvCutoffSqrTab[256];

static void icvInitStereoConstTabs() {
	static volatile int initialized = 0;
	if ( !initialized ) {
		int i;
		for ( i = 0; i < 512; i++ ) {
			icvTruncTab[i] = (uchar)MIN(MAX(i - 255, 0), 255);
		}
		for ( i = 0; i < 256; i++ ) {
			icvCutoffSqrTab[i] = MIN(i * i, CUTOFF);
		}
		initialized = 1;
	}
}

static void icvInitStereoTabs( CvStereoGCState2* state2 ) {
	int i, K = state2->K;

	for ( i = 0; i <= CUTOFF; i++ ) {
		state2->dataCostFuncTab[i] = MIN(i * DENOMINATOR - K, 0);
	}

	for ( i = 0; i < CUTOFF * 2 + 1; i++ ) {
		state2->smoothnessR[i] = MIN(abs(i - CUTOFF), state2->interactionRadius);
	}

	for ( i = 0; i < 512; i++ ) {
		int diff = abs(i - 255);
		state2->smoothnessGrayDiff[i] = diff < state2->Ithreshold ? state2->lambda1 : state2->lambda2;
	}
}


static int icvGCResizeOrphansBuf( GCVtx** & orphans, int norphans ) {
	int i, newNOrphans = MAX(norphans * 3 / 2, 256);
	GCVtx** newOrphans = (GCVtx**)cvAlloc( newNOrphans * sizeof(orphans[0]) );
	for ( i = 0; i < norphans; i++ ) {
		newOrphans[i] = orphans[i];
	}
	cvFree( &orphans );
	orphans = newOrphans;
	return newNOrphans;
}

static int64 icvGCMaxFlow( GCVtx* vtx, int nvtx, GCEdge* edges, GCVtx** & _orphans, int& _maxOrphans ) {
	const int TERMINAL = -1, ORPHAN = -2;
	GCVtx stub, *nilNode = &stub, *first = nilNode, *last = nilNode;
	int i, k;
	int curr_ts = 0;
	int64 flow = 0;
	int norphans = 0, maxOrphans = _maxOrphans;
	GCVtx** orphans = _orphans;
	stub.next = nilNode;

	// initialize the active queue and the graph vertices
	for ( i = 0; i < nvtx; i++ ) {
		GCVtx* v = vtx + i;
		v->ts = 0;
		if ( v->weight != 0 ) {
			last = last->next = v;
			v->dist = 1;
			v->parent = TERMINAL;
			v->t = v->weight < 0;
		} else {
			v->parent = 0;
		}
	}

	first = first->next;
	last->next = nilNode;
	nilNode->next = 0;

	// run the search-path -> augment-graph -> restore-trees loop
	for (;;) {
		GCVtx* v, *u;
		int e0 = -1, ei = 0, ej = 0, min_weight, weight;
		uchar vt;

		// grow S & T search trees, find an edge connecting them
		while ( first != nilNode ) {
			v = first;
			if ( v->parent ) {
				vt = v->t;
				for ( ei = v->first; ei != 0; ei = edges[ei].next ) {
					if ( edges[ei^vt].weight == 0 ) {
						continue;
					}
					u = edges[ei].dst;
					if ( !u->parent ) {
						u->t = vt;
						u->parent = ei ^ 1;
						u->ts = v->ts;
						u->dist = v->dist + 1;
						if ( !u->next ) {
							u->next = nilNode;
							last = last->next = u;
						}
						continue;
					}

					if ( u->t != vt ) {
						e0 = ei ^ vt;
						break;
					}

					if ( u->dist > v->dist + 1 && u->ts <= v->ts ) {
						// reassign the parent
						u->parent = ei ^ 1;
						u->ts = v->ts;
						u->dist = v->dist + 1;
					}
				}
				if ( e0 > 0 ) {
					break;
				}
			}
			// exclude the vertex from the active list
			first = first->next;
			v->next = 0;
		}

		if ( e0 <= 0 ) {
			break;
		}

		// find the minimum edge weight along the path
		min_weight = edges[e0].weight;
		assert( min_weight > 0 );
		// k = 1: source tree, k = 0: destination tree
		for ( k = 1; k >= 0; k-- ) {
			for ( v = edges[e0^k].dst;; v = edges[ei].dst ) {
				if ( (ei = v->parent) < 0 ) {
					break;
				}
				weight = edges[ei^k].weight;
				min_weight = MIN(min_weight, weight);
				assert( min_weight > 0 );
			}
			weight = abs(v->weight);
			min_weight = MIN(min_weight, weight);
			assert( min_weight > 0 );
		}

		// modify weights of the edges along the path and collect orphans
		edges[e0].weight -= min_weight;
		edges[e0^1].weight += min_weight;
		flow += min_weight;

		// k = 1: source tree, k = 0: destination tree
		for ( k = 1; k >= 0; k-- ) {
			for ( v = edges[e0^k].dst;; v = edges[ei].dst ) {
				if ( (ei = v->parent) < 0 ) {
					break;
				}
				edges[ei^(k^1)].weight += min_weight;
				if ( (edges[ei^k].weight -= min_weight) == 0 ) {
					if ( norphans >= maxOrphans ) {
						maxOrphans = icvGCResizeOrphansBuf( orphans, norphans );
					}
					orphans[norphans++] = v;
					v->parent = ORPHAN;
				}
			}

			v->weight = (short)(v->weight + min_weight * (1 - k * 2));
			if ( v->weight == 0 ) {
				if ( norphans >= maxOrphans ) {
					maxOrphans = icvGCResizeOrphansBuf( orphans, norphans );
				}
				orphans[norphans++] = v;
				v->parent = ORPHAN;
			}
		}

		// restore the search trees by finding new parents for the orphans
		curr_ts++;
		while ( norphans > 0 ) {
			GCVtx* v = orphans[--norphans];
			int d, min_dist = INT_MAX;
			e0 = 0;
			vt = v->t;

			for ( ei = v->first; ei != 0; ei = edges[ei].next ) {
				if ( edges[ei^(vt^1)].weight == 0 ) {
					continue;
				}
				u = edges[ei].dst;
				if ( u->t != vt || u->parent == 0 ) {
					continue;
				}
				// compute the distance to the tree root
				for ( d = 0;; ) {
					if ( u->ts == curr_ts ) {
						d += u->dist;
						break;
					}
					ej = u->parent;
					d++;
					if ( ej < 0 ) {
						if ( ej == ORPHAN ) {
							d = INT_MAX - 1;
						} else {
							u->ts = curr_ts;
							u->dist = 1;
						}
						break;
					}
					u = edges[ej].dst;
				}

				// update the distance
				if ( ++d < INT_MAX ) {
					if ( d < min_dist ) {
						min_dist = d;
						e0 = ei;
					}
					for ( u = edges[ei].dst; u->ts != curr_ts; u = edges[u->parent].dst ) {
						u->ts = curr_ts;
						u->dist = --d;
					}
				}
			}

			if ( (v->parent = e0) > 0 ) {
				v->ts = curr_ts;
				v->dist = min_dist;
				continue;
			}

			/* no parent is found */
			v->ts = 0;
			for ( ei = v->first; ei != 0; ei = edges[ei].next ) {
				u = edges[ei].dst;
				ej = u->parent;
				if ( u->t != vt || !ej ) {
					continue;
				}
				if ( edges[ei^(vt^1)].weight && !u->next ) {
					u->next = nilNode;
					last = last->next = u;
				}
				if ( ej > 0 && edges[ej].dst == v ) {
					if ( norphans >= maxOrphans ) {
						maxOrphans = icvGCResizeOrphansBuf( orphans, norphans );
					}
					orphans[norphans++] = u;
					u->parent = ORPHAN;
				}
			}
		}
	}

	_orphans = orphans;
	_maxOrphans = maxOrphans;

	return flow;
}


CvStereoGCState* cvCreateStereoGCState( int numberOfDisparities, int maxIters ) {
	CvStereoGCState* state = 0;

	state = (CvStereoGCState*)cvAlloc( sizeof(*state) );
	memset( state, 0, sizeof(*state) );
	state->minDisparity = 0;
	state->numberOfDisparities = numberOfDisparities;
	state->maxIters = maxIters <= 0 ? 3 : maxIters;
	state->Ithreshold = 5;
	state->interactionRadius = 1;
	state->K = state->lambda = state->lambda1 = state->lambda2 = -1.f;
	state->occlusionCost = OCCLUSION_PENALTY;

	return state;
}

void cvReleaseStereoGCState( CvStereoGCState** _state ) {
	CvStereoGCState* state;

	if ( !_state && !*_state ) {
		return;
	}

	state = *_state;
	cvReleaseMat( &state->left );
	cvReleaseMat( &state->right );
	cvReleaseMat( &state->ptrLeft );
	cvReleaseMat( &state->ptrRight );
	cvReleaseMat( &state->vtxBuf );
	cvReleaseMat( &state->edgeBuf );
	cvFree( _state );
}

// ||I(x) - J(x')|| =
// min(CUTOFF,
//   min(
//     max(
//       max(minJ(x') - I(x), 0),
//       max(I(x) - maxJ(x'), 0)),
//     max(
//       max(minI(x) - J(x'), 0),
//       max(J(x') - maxI(x), 0)))**2) ==
// min(CUTOFF,
//   min(
//       max(minJ(x') - I(x), 0) +
//       max(I(x) - maxJ(x'), 0),
//
//       max(minI(x) - J(x'), 0) +
//       max(J(x') - maxI(x), 0)))**2)
// where (I, minI, maxI) and
//       (J, minJ, maxJ) are stored as interleaved 3-channel images.
// minI, maxI are computed from I,
// minJ, maxJ are computed from J - see icvInitGraySubPix.
static inline int icvDataCostFuncGraySubpix( const uchar* a, const uchar* b ) {
	int va = a[0], vb = b[0];
	int da = icvTruncTab[b[1] - va + 255] + icvTruncTab[va - b[2] + 255];
	int db = icvTruncTab[a[1] - vb + 255] + icvTruncTab[vb - a[2] + 255];
	return icvCutoffSqrTab[MIN(da, db)];
}

static inline int icvSmoothnessCostFunc( int da, int db, int maxR, const int* stabR, int scale ) {
	return da == db ? 0 : (da == OCCLUDED || db == OCCLUDED ? maxR : stabR[da - db]) * scale;
}

static void icvInitGraySubpix( const CvMat* left, const CvMat* right,
							   CvMat* left3, CvMat* right3 ) {
	int k, x, y, rows = left->rows, cols = left->cols;

	for ( k = 0; k < 2; k++ ) {
		const CvMat* src = k == 0 ? left : right;
		CvMat* dst = k == 0 ? left3 : right3;
		int sstep = src->step;

		for ( y = 0; y < rows; y++ ) {
			const uchar* sptr = src->data.ptr + sstep * y;
			const uchar* sptr_prev = y > 0 ? sptr - sstep : sptr;
			const uchar* sptr_next = y < rows - 1 ? sptr + sstep : sptr;
			uchar* dptr = dst->data.ptr + dst->step * y;
			int v_prev = sptr[0];

			for ( x = 0; x < cols; x++, dptr += 3 ) {
				int v = sptr[x], v1, minv = v, maxv = v;

				v1 = (v + v_prev) / 2;
				minv = MIN(minv, v1); maxv = MAX(maxv, v1);
				v1 = (v + sptr_prev[x]) / 2;
				minv = MIN(minv, v1); maxv = MAX(maxv, v1);
				v1 = (v + sptr_next[x]) / 2;
				minv = MIN(minv, v1); maxv = MAX(maxv, v1);
				if ( x < cols - 1 ) {
					v1 = (v + sptr[x+1]) / 2;
					minv = MIN(minv, v1); maxv = MAX(maxv, v1);
				}
				v_prev = v;
				dptr[0] = (uchar)v;
				dptr[1] = (uchar)minv;
				dptr[2] = (uchar)maxv;
			}
		}
	}
}

// Optimal K is computed as avg_x(k-th-smallest_d(||I(x)-J(x+d)||)),
// where k = number_of_disparities*0.25.
static float
icvComputeK( CvStereoGCState* state ) {
	int x, y, x1, d, i, j, rows = state->left->rows, cols = state->left->cols, n = 0;
	int mind = state->minDisparity, nd = state->numberOfDisparities, maxd = mind + nd;
	int k = MIN(MAX((nd + 2) / 4, 3), nd);
	int* arr = (int*)cvStackAlloc(k * sizeof(arr[0])), delta, t, sum = 0;

	for ( y = 0; y < rows; y++ ) {
		const uchar* lptr = state->left->data.ptr + state->left->step * y;
		const uchar* rptr = state->right->data.ptr + state->right->step * y;

		for ( x = 0; x < cols; x++ ) {
			for ( d = maxd - 1, i = 0; d >= mind; d-- ) {
				x1 = x - d;
				if ( (unsigned)x1 >= (unsigned)cols ) {
					continue;
				}
				delta = icvDataCostFuncGraySubpix( lptr + x * 3, rptr + x1 * 3 );
				if ( i < k ) {
					arr[i++] = delta;
				} else
					for ( i = 0; i < k; i++ )
						if ( delta < arr[i] ) {
							CV_SWAP( arr[i], delta, t );
						}
			}
			delta = arr[0];
			for ( j = 1; j < i; j++ ) {
				delta = MAX(delta, arr[j]);
			}
			sum += delta;
			n++;
		}
	}

	return (float)sum / n;
}

static int64 icvComputeEnergy( const CvStereoGCState* state, const CvStereoGCState2* state2,
							   bool allOccluded ) {
	int x, y, rows = state->left->rows, cols = state->left->cols;
	int64 E = 0;
	const int* dtab = state2->dataCostFuncTab;
	int maxR = state2->interactionRadius;
	const int* stabR = state2->smoothnessR + CUTOFF;
	const int* stabI = state2->smoothnessGrayDiff + 255;
	const uchar* left = state->left->data.ptr;
	const uchar* right = state->right->data.ptr;
	short* dleft = state->dispLeft->data.s;
	short* dright = state->dispRight->data.s;
	int step = state->left->step;
	int dstep = (int)(state->dispLeft->step / sizeof(short));

	assert( state->left->step == state->right->step &&
			state->dispLeft->step == state->dispRight->step );

	if ( allOccluded ) {
		return (int64)OCCLUSION_PENALTY * rows * cols * 2;
	}

	for ( y = 0; y < rows; y++, left += step, right += step, dleft += dstep, dright += dstep ) {
		for ( x = 0; x < cols; x++ ) {
			int d = dleft[x], x1, d1;
			if ( d == OCCLUDED ) {
				E += OCCLUSION_PENALTY;
			} else {
				x1 = x + d;
				if ( (unsigned)x1 >= (unsigned)cols ) {
					continue;
				}
				d1 = dright[x1];
				if ( d == -d1 ) {
					E += dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )];
				}
			}

			if ( x < cols - 1 ) {
				d1 = dleft[x+1];
				E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[left[x*3] - left[x*3+3]] );
			}
			if ( y < rows - 1 ) {
				d1 = dleft[x+dstep];
				E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[left[x*3] - left[x*3+step]] );
			}

			d = dright[x];
			if ( d == OCCLUDED ) {
				E += OCCLUSION_PENALTY;
			}

			if ( x < cols - 1 ) {
				d1 = dright[x+1];
				E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[right[x*3] - right[x*3+3]] );
			}
			if ( y < rows - 1 ) {
				d1 = dright[x+dstep];
				E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[right[x*3] - right[x*3+step]] );
			}
			assert( E >= 0 );
		}
	}

	return E;
}

static inline void icvAddEdge( GCVtx* x, GCVtx* y, GCEdge* edgeBuf, int nedges, int w, int rw ) {
	GCEdge* xy = edgeBuf + nedges, *yx = xy + 1;

	assert( x != 0 && y != 0 );
	xy->dst = y;
	xy->next = x->first;
	xy->weight = (short)w;
	x->first = nedges;

	yx->dst = x;
	yx->next = y->first;
	yx->weight = (short)rw;
	y->first = nedges + 1;
}

static inline int icvAddTWeights( GCVtx* vtx, int sourceWeight, int sinkWeight ) {
	int w = vtx->weight;
	if ( w > 0 ) {
		sourceWeight += w;
	} else {
		sinkWeight -= w;
	}
	vtx->weight = (short)(sourceWeight - sinkWeight);
	return MIN(sourceWeight, sinkWeight);
}

static inline int icvAddTerm( GCVtx* x, GCVtx* y, int A, int B, int C, int D,
							  GCEdge* edgeBuf, int& nedges ) {
	int dE = 0, w;

	assert(B - A + C - D >= 0);
	if ( B < A ) {
		dE += icvAddTWeights(x, D, B);
		dE += icvAddTWeights(y, 0, A - B);
		if ( (w = B - A + C - D) != 0 ) {
			icvAddEdge( x, y, edgeBuf, nedges, 0, w );
			nedges += 2;
		}
	} else if ( C < D ) {
		dE += icvAddTWeights(x, D, A + D - C);
		dE += icvAddTWeights(y, 0, C - D);
		if ( (w = B - A + C - D) != 0 ) {
			icvAddEdge( x, y, edgeBuf, nedges, w, 0 );
			nedges += 2;
		}
	} else {
		dE += icvAddTWeights(x, D, A);
		if ( B != A || C != D ) {
			icvAddEdge( x, y, edgeBuf, nedges, B - A, C - D );
			nedges += 2;
		}
	}
	return dE;
}

static int64 icvAlphaExpand( int64 Eprev, int alpha, CvStereoGCState* state, CvStereoGCState2* state2 ) {
	GCVtx* var, *var1;
	int64 E = 0;
	int delta, E00 = 0, E0a = 0, Ea0 = 0, Eaa = 0;
	int k, a, d, d1, x, y, x1, y1, rows = state->left->rows, cols = state->left->cols;
	int nvtx = 0, nedges = 2;
	GCVtx* vbuf = (GCVtx*)state->vtxBuf->data.ptr;
	GCEdge* ebuf = (GCEdge*)state->edgeBuf->data.ptr;
	int maxR = state2->interactionRadius;
	const int* dtab = state2->dataCostFuncTab;
	const int* stabR = state2->smoothnessR + CUTOFF;
	const int* stabI = state2->smoothnessGrayDiff + 255;
	const uchar* left0 = state->left->data.ptr;
	const uchar* right0 = state->right->data.ptr;
	short* dleft0 = state->dispLeft->data.s;
	short* dright0 = state->dispRight->data.s;
	GCVtx** pleft0 = (GCVtx**)state->ptrLeft->data.ptr;
	GCVtx** pright0 = (GCVtx**)state->ptrRight->data.ptr;
	int step = state->left->step;
	int dstep = (int)(state->dispLeft->step / sizeof(short));
	int pstep = (int)(state->ptrLeft->step / sizeof(GCVtx*));
	int aa[] = { alpha, -alpha };

	//double t = (double)cvGetTickCount();

	assert( state->left->step == state->right->step &&
			state->dispLeft->step == state->dispRight->step &&
			state->ptrLeft->step == state->ptrRight->step );
	for ( k = 0; k < 2; k++ ) {
		ebuf[k].dst = 0;
		ebuf[k].next = 0;
		ebuf[k].weight = 0;
	}

	for ( y = 0; y < rows; y++ ) {
		const uchar* left = left0 + step * y;
		const uchar* right = right0 + step * y;
		const short* dleft = dleft0 + dstep * y;
		const short* dright = dright0 + dstep * y;
		GCVtx** pleft = pleft0 + pstep * y;
		GCVtx** pright = pright0 + pstep * y;
		const uchar* lr[] = { left, right };
		const short* dlr[] = { dleft, dright };
		GCVtx** plr[] = { pleft, pright };

		for ( k = 0; k < 2; k++ ) {
			a = aa[k];
			for ( y1 = y + (y > 0); y1 <= y + (y < rows - 1); y1++ ) {
				const short* disp = (k == 0 ? dleft0 : dright0) + y1 * dstep;
				GCVtx** ptr = (k == 0 ? pleft0 : pright0) + y1 * pstep;
				for ( x = 0; x < cols; x++ ) {
					GCVtx* v = ptr[x] = &vbuf[nvtx++];
					v->first = 0;
					v->weight = disp[x] == (short)(OCCLUDED ? -OCCLUSION_PENALTY2 : 0);
				}
			}
		}

		for ( x = 0; x < cols; x++ ) {
			d = dleft[x];
			x1 = x + d;
			var = pleft[x];

			// (left + x, right + x + d)
			if ( d != alpha && d != OCCLUDED && (unsigned)x1 < (unsigned)cols ) {
				var1 = pright[x1];
				d1 = dright[x1];
				if ( d == -d1 ) {
					assert( var1 != 0 );
					delta = IS_BLOCKED(alpha, d) ? INFINITY : 0;
					//add inter edge
					E += icvAddTerm( var, var1,
									 dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )],
									 delta, delta, 0, ebuf, nedges );
				} else if ( IS_BLOCKED(alpha, d) ) {
					E += icvAddTerm( var, var1, 0, INFINITY, 0, 0, ebuf, nedges );
				}
			}

			// (left + x, right + x + alpha)
			x1 = x + alpha;
			if ( (unsigned)x1 < (unsigned)cols ) {
				var1 = pright[x1];
				d1 = dright[x1];

				E0a = IS_BLOCKED(d, alpha) ? INFINITY : 0;
				Ea0 = IS_BLOCKED(-d1, alpha) ? INFINITY : 0;
				Eaa = dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )];
				E += icvAddTerm( var, var1, 0, E0a, Ea0, Eaa, ebuf, nedges );
			}

			// smoothness
			for ( k = 0; k < 2; k++ ) {
				GCVtx** p = plr[k];
				const short* disp = dlr[k];
				const uchar* img = lr[k] + x * 3;
				int scale;
				var = p[x];
				d = disp[x];
				a = aa[k];

				if ( x < cols - 1 ) {
					var1 = p[x+1];
					d1 = disp[x+1];
					scale = stabI[img[0] - img[3]];
					E0a = icvSmoothnessCostFunc( d, a, maxR, stabR, scale );
					Ea0 = icvSmoothnessCostFunc( a, d1, maxR, stabR, scale );
					E00 = icvSmoothnessCostFunc( d, d1, maxR, stabR, scale );
					E += icvAddTerm( var, var1, E00, E0a, Ea0, 0, ebuf, nedges );
				}

				if ( y < rows - 1 ) {
					var1 = p[x+pstep];
					d1 = disp[x+dstep];
					scale = stabI[img[0] - img[step]];
					E0a = icvSmoothnessCostFunc( d, a, maxR, stabR, scale );
					Ea0 = icvSmoothnessCostFunc( a, d1, maxR, stabR, scale );
					E00 = icvSmoothnessCostFunc( d, d1, maxR, stabR, scale );
					E += icvAddTerm( var, var1, E00, E0a, Ea0, 0, ebuf, nedges );
				}
			}

			// visibility term
			if ( d != OCCLUDED && IS_BLOCKED(alpha, -d)) {
				x1 = x + d;
				if ( (unsigned)x1 < (unsigned)cols ) {
					if ( d != -dleft[x1] ) {
						var1 = pleft[x1];
						E += icvAddTerm( var, var1, 0, INFINITY, 0, 0, ebuf, nedges );
					}
				}
			}
		}
	}

	//t = (double)cvGetTickCount() - t;
	ebuf[0].weight = ebuf[1].weight = 0;
	E += icvGCMaxFlow( vbuf, nvtx, ebuf, state2->orphans, state2->maxOrphans );

	if ( E < Eprev ) {
		for ( y = 0; y < rows; y++ ) {
			short* dleft = dleft0 + dstep * y;
			short* dright = dright0 + dstep * y;
			GCVtx** pleft = pleft0 + pstep * y;
			GCVtx** pright = pright0 + pstep * y;
			for ( x = 0; x < cols; x++ ) {
				GCVtx* var = pleft[x];
				if ( var && var->parent && var->t ) {
					dleft[x] = (short)alpha;
				}

				var = pright[x];
				if ( var && var->parent && var->t ) {
					dright[x] = (short) - alpha;
				}
			}
		}
	}

	return MIN(E, Eprev);
}


CV_IMPL void cvFindStereoCorrespondenceGC( const CvArr* _left, const CvArr* _right,
		CvArr* _dispLeft, CvArr* _dispRight, CvStereoGCState* state, int useDisparityGuess ) {
	CvStereoGCState2 state2;
	state2.orphans = 0;
	state2.maxOrphans = 0;

	CvMat lstub, *left = cvGetMat( _left, &lstub );
	CvMat rstub, *right = cvGetMat( _right, &rstub );
	CvMat dlstub, *dispLeft = cvGetMat( _dispLeft, &dlstub );
	CvMat drstub, *dispRight = cvGetMat( _dispRight, &drstub );
	CvSize size;
	int iter, i, nZeroExpansions = 0;
	CvRNG rng = cvRNG(-1);
	int* disp;
	CvMat _disp;
	int64 E;

	CV_Assert( state != 0 );
	CV_Assert( CV_ARE_SIZES_EQ(left, right) && CV_ARE_TYPES_EQ(left, right) &&
			   CV_MAT_TYPE(left->type) == CV_8UC1 );
	CV_Assert( !dispLeft ||
			   (CV_ARE_SIZES_EQ(dispLeft, left) && CV_MAT_CN(dispLeft->type) == 1) );
	CV_Assert( !dispRight ||
			   (CV_ARE_SIZES_EQ(dispRight, left) && CV_MAT_CN(dispRight->type) == 1) );

	size = cvGetSize(left);
	if ( !state->left || state->left->width != size.width || state->left->height != size.height ) {
		int pcn = (int)(sizeof(GCVtx*) / sizeof(int));
		int vcn = (int)(sizeof(GCVtx) / sizeof(int));
		int ecn = (int)(sizeof(GCEdge) / sizeof(int));
		cvReleaseMat( &state->left );
		cvReleaseMat( &state->right );
		cvReleaseMat( &state->ptrLeft );
		cvReleaseMat( &state->ptrRight );
		cvReleaseMat( &state->dispLeft );
		cvReleaseMat( &state->dispRight );

		state->left = cvCreateMat( size.height, size.width, CV_8UC3 );
		state->right = cvCreateMat( size.height, size.width, CV_8UC3 );
		state->dispLeft = cvCreateMat( size.height, size.width, CV_16SC1 );
		state->dispRight = cvCreateMat( size.height, size.width, CV_16SC1 );
		state->ptrLeft = cvCreateMat( size.height, size.width, CV_32SC(pcn) );
		state->ptrRight = cvCreateMat( size.height, size.width, CV_32SC(pcn) );
		state->vtxBuf = cvCreateMat( 1, size.height * size.width * 2, CV_32SC(vcn) );
		state->edgeBuf = cvCreateMat( 1, size.height * size.width * 12 + 16, CV_32SC(ecn) );
	}

	if ( !useDisparityGuess ) {
		cvSet( state->dispLeft, cvScalarAll(OCCLUDED));
		cvSet( state->dispRight, cvScalarAll(OCCLUDED));
	} else {
		CV_Assert( dispLeft && dispRight );
		cvConvert( dispLeft, state->dispLeft );
		cvConvert( dispRight, state->dispRight );
	}

	state2.Ithreshold = state->Ithreshold;
	state2.interactionRadius = state->interactionRadius;
	state2.lambda = cvRound(state->lambda * DENOMINATOR);
	state2.lambda1 = cvRound(state->lambda1 * DENOMINATOR);
	state2.lambda2 = cvRound(state->lambda2 * DENOMINATOR);
	state2.K = cvRound(state->K * DENOMINATOR);

	icvInitStereoConstTabs();
	icvInitGraySubpix( left, right, state->left, state->right );
	disp = (int*)cvStackAlloc( state->numberOfDisparities * sizeof(disp[0]) );
	_disp = cvMat( 1, state->numberOfDisparities, CV_32S, disp );
	cvRange( &_disp, state->minDisparity, state->minDisparity + state->numberOfDisparities );
	cvRandShuffle( &_disp, &rng );

	if ( state2.lambda < 0 && (state2.K < 0 || state2.lambda1 < 0 || state2.lambda2 < 0) ) {
		float L = icvComputeK(state) * 0.2f;
		state2.lambda = cvRound(L * DENOMINATOR);
	}

	if ( state2.K < 0 ) {
		state2.K = state2.lambda * 5;
	}
	if ( state2.lambda1 < 0 ) {
		state2.lambda1 = state2.lambda * 3;
	}
	if ( state2.lambda2 < 0 ) {
		state2.lambda2 = state2.lambda;
	}

	icvInitStereoTabs( &state2 );

	E = icvComputeEnergy( state, &state2, !useDisparityGuess );
	for ( iter = 0; iter < state->maxIters; iter++ ) {
		for ( i = 0; i < state->numberOfDisparities; i++ ) {
			int alpha = disp[i];
			int64 Enew = icvAlphaExpand( E, -alpha, state, &state2 );
			if ( Enew < E ) {
				nZeroExpansions = 0;
				E = Enew;
			} else if ( ++nZeroExpansions >= state->numberOfDisparities ) {
				break;
			}
		}
	}

	if ( dispLeft ) {
		cvConvert( state->dispLeft, dispLeft );
	}
	if ( dispRight ) {
		cvConvert( state->dispRight, dispRight );
	}

	cvFree( &state2.orphans );
}
